#include <stdint.h>
#include "motor.h"

#define VERT_MIN 128            // Dead zone so ISRs play nicely

volatile uint8_t vert_dir = 1;      // 1 - up, 0 - down
volatile uint8_t vert_pwm_high = 0; // 1 - on, 0 - off : software PWM
volatile uint16_t vert_duty = VERT_MIN; // (127<<7) * duty_cycle

void initialize_motors (void) {
    P1DIR = (BIT0 + BIT1 +       // V-fan output
             BIT2 + BIT3 +       // L-fan output
             BIT4 + BIT5);       // R-fan output
    P1OUT |= BIT0 + BIT1;
    P1SEL |= (BIT2 + BIT3 +      // L-fan PWM output
              BIT4 + BIT5);      // R-fan PWM output

    ////////////////////////////////////////
    // Timer A0
    ////////////////////////////////////////
    TA0CTL   |= TASSEL_2 + MC_1 + TACLR + ID_3;
    TA0EX0   = TAIDEX_3; // With the expansion and ID3, we will be dividing
                         // the clock by 2^4
      
    TA0CCR0  = MOTOR_PWM_PERIOD; // PWM Period

    // Left Fan
    TA0CCTL1 = OUTMOD_3;        // CCR1 set/reset 0___|---PERIOD
    TA0CCR1  = 0;               // LFAN_H PERIOD = 0 (Init high)
    TA0CCTL2 = OUTMOD_7;	// CCR2 reset/set 0---|___PERIOD
    TA0CCR2  = MOTOR_PWM_PERIOD; // LFAN_L PERIOD = MOTOR_PWM_PERIOD (Init high)

    // Right Fan
    TA0CCTL3 = OUTMOD_3;        // CCR1 set/reset 0___|---PERIOD
    TA0CCR3  = 0;               // RFAN_H PERIOD = 0 (Init high)
    TA0CCTL4 = OUTMOD_7;        // CCR4 reset/set 0---|___PERIOD
    TA0CCR4  = MOTOR_PWM_PERIOD; // RFAN_L PERIOD = MOTOR_PWM_PERIOD (Init high) 

    return;
}

void set_motor_speed(int8_t left, int8_t right, int8_t vert) {
    // Left Fan
    if( left > 0 ){ //forward
        TA0CCR1  = left << 7;          // L_high, 0 (fan on) on PWM cycle 
        TA0CCR2  = MOTOR_PWM_PERIOD;   // L_low, always 1
    }else{// reverse
        TA0CCR1  = 0;
        TA0CCR2  = (MOTOR_PWM_PERIOD-((-left)<<7));
    }

    // Right Fan
    if( right > 0 ){ //forward
        TA0CCR3  = right << 7; // R_high, goes to 0 during pwm cycle
        TA0CCR4  = MOTOR_PWM_PERIOD; // R_low, always 1
    }else{ // reverse
        TA0CCR3  = 0; // R_high, 1 
        TA0CCR4  = MOTOR_PWM_PERIOD-((-right)<<7); // R_low, goes to 0 during PWM cycle
    }

    // Because there are problems running timerA and uart interrupts together, we can only
    // set the vertical motor to forward, reverse, or off
    uint8_t p1_buffer = P1OUT & ~(BIT0 | BIT1); // mask out two low bits
    if (vert < 0) {
        p1_buffer |= BIT0;      // going down
    } else if (vert > 0) {
        p1_buffer |= BIT1;      // going up
    } else {
        p1_buffer |= BIT0 + BIT1; // off
    }
    P1OUT = p1_buffer;
}

